import numpy as np
from scipy.spatial.transform import Rotation as R

def set_pose(iter_pose):
    iter_pose = poses[iter]
    init_R = iter_pose[:9].reshape(3, 3)
    init_t = iter_pose[9:] * cfg.geometry_unit_in_meter
    iter_pose = Pose.from_Rt(init_R, init_t)    
    return iter_pose


# 将位姿保存为[1, 12]的数组 到txt中
